#include <stdio.h>
#include <math.h>
#include <string.h>
#include <unistd.h>

#include "control.h"
#include "misc.h"

//比例参数 从机器人发过来的数据乘的倍数 设置该值的目的是方便控制参数调试
#define XRATIO  1000        //车体X
#define ZRATIO  1000        //车体Y
#define ANGLERATIO 1000000  //车体角度
#define DISRATIO 10         //距离传感器
#define CAMERATIO 10000      //摄像头
#define DISSENRATIO 300

//最大车轮速度
#define MAXWHEELSPEED 10
//最大手臂末端移动速度 用在视觉引导抓取上
#define MAXARMENDSPEED 0.02

//位置、角度和视觉引导控制死区
#define DEADPOS     0.014
#define DEADANGLE   0.01
#define DEADCAMERA  0.001

//机械臂相关参数
#define ARMXOFFSET  0.156
#define ARM1HLENGTH 0.033
#define ARM1VLENGTH 0.147
#define ARM2LENGTH  0.155
#define ARM3LENGTH  0.135
#define ARM4LENGTH  0.081
#define ARM5LENGTH  0.105

//同时控制车和机械臂时 机械臂末端相对车x的偏移量 
#define POSOFFSET 0.4

//机械夹爪力
#define GRIPPERFORCE 400.0
//机械夹爪最大、最小间隔
#define MAXGAP 0.029
#define MINGAP 0.021

//车体运动控制变量，分别是当前值和目标值  
struct variables cur_var,tar_var;
//控制器变量
struct controller con;
//车轮速度
struct wheelspeed ws;
//机械臂关节空间变量
struct arm arm1;
//机械臂笛卡尔空间变量
struct armvar armtar;
//视觉引导抓取变量
struct camera cma_cur,cma_tar;
//机械臂夹爪变量
struct gripper gripper;

//距离传感器变量，避障用
static float ds_values[3];

static float temp_tar_angle = 0;
static float distance = 0;

//控制标志位
static int control_flag = 0;
static int armwithpos_flag = 0;
static int visualgrap_flag = 0;

/***************************************
 * 函数功能: 从WEBOTS发来的结构体中提取相应的数据
 * *************************************/
void recVarFromComm(SGAGV_TC*tc)
{
    int32_t temp;

    temp = tc->gpsX;
    cur_var.gpsX = (float)temp/XRATIO;

    temp = tc->gpsZ;
    cur_var.gpsZ = (float)temp/ZRATIO;

    cur_var.angle = tc->angle;
    cur_var.angle /= ANGLERATIO;

    ds_values[0] = (float)tc->leftds/DISRATIO;
    ds_values[1] = (float)tc->middleds/DISRATIO;
    ds_values[2] = (float)tc->rightds/DISRATIO;

    temp = tc->cameraX;
    cma_cur.cameraX = (float)temp/CAMERATIO;

    temp = tc->cameraY;
    cma_cur.cameraY = (float)temp/CAMERATIO;

    temp = tc->cameraZ;
    cma_cur.cameraZ = (float)temp/CAMERATIO;

    temp = tc->force;
    gripper.force = temp;

    //printf("force = %f \n",gripper.force);
}

/***************************************
 * 函数功能: 初始化控制器参数
 * 其他：在这里设置目标点和目标角度以及调参
 * *************************************/
void initControl(void)
{
    con.angle_P = 10;
    con.angle_D = 0;
    con.trans_P = 10;
    con.trans_D = 0;
    con.camera_I = 0.02;
    con.camera_D = 0;

    tar_var.gpsZ = 0;
    tar_var.gpsX = 0;
    tar_var.angle = 0;
    temp_tar_angle = 0;

    ws.left_front = 0;
    ws.left_rear = 0;
    ws.right_front = 0;
    ws.right_rear = 0;

    control_flag = 0;

    cma_tar.cameraX = 0;
    cma_tar.cameraY = -0.02;
    cma_tar.cameraZ = -0.05;

    gripper.gap = 0.029;
}
/***************************************
 * 函数功能: 初始化机械臂关节角
 * *************************************/
void initArm(void)
{
    arm1.joint1 = 0;
    arm1.joint2 = 1.57;
    arm1.joint3 = -2.635;
    arm1.joint4 = 1.78;
    arm1.joint5 = 0;
}
/***************************************
 * 函数功能: 设置机械臂关节角
 * *************************************/
void setArmJoint(float joint1,float joint2,float joint3,float joint4,float joint5)
{
    arm1.joint1 = joint1;
    arm1.joint2 = joint2;
    arm1.joint3 = joint3;
    arm1.joint4 = joint4;
    arm1.joint5 = joint5;
}
/***************************************
 * 函数功能: 角度控制 PD控制
 * 返回值：到达目标角度返回1 否则返回0
 * *************************************/
int angleControl(float tar)
{
    float temp,ret = 0;

    temp = angleDifference(tar,cur_var.angle);

    if(fabs(temp) >= DEADANGLE)
        temp *= con.angle_P;
    else
    {
        temp = 0;
        ret = 1;
    }

    //printf("temp = %f cur_var.angle=%f\r\n",temp,cur_var.angle);    

    ws.right_front += temp;
    ws.right_rear += temp;
    ws.left_front += -temp;
    ws.left_rear += -temp;
    limitWheelSpeed();

    return ret;
}
/***************************************
 * 函数功能: 位置控制 PD控制
 * 参数：offset 距离目标的距离
 * 返回值：到达目标位置返回1 否则返回0
 * *************************************/
int positionControl(float offset)
{
    float temp;
    int ret = 0;

    distance = sqrt(powf(tar_var.gpsX-cur_var.gpsX,2)+powf(tar_var.gpsZ-cur_var.gpsZ,2));

    if(distance>offset)
        distance -= offset;
    else
        distance = 0;

    if(fabs(distance) >= DEADPOS)
        temp = distance*con.trans_P;
    else
    {
        temp = 0;
        ret = 1;
    }

    ws.right_front = ws.right_rear = ws.left_front = ws.left_rear = temp;
    limitWheelSpeed();
    return ret;
}

/***************************************
 * 函数功能: 避障
 * *************************************/
void obstacleAvoidance(void)
{
    float threshold = 250;
    float near_th = 175;
    unsigned char al = (ds_values[0] < threshold), am = (ds_values[1] < threshold), ar = (ds_values[2] < threshold);
    unsigned char condition = (al<<2) | (am<<1) | ar;

    float min = ds_values[0] < ds_values[1] ? ds_values[0] : ds_values[1];
          min = min < ds_values[2] ? min : ds_values[2];

    if(distance < min/DISSENRATIO) //表示到目标的距离小于到障碍的距离 无需避障
        return ;


    switch (condition)
    {
        case 0x4:   //0x4表明左边距离传感器测量的距离小于threshold，0x6表明左边和中间的距离传感器测距都小于threshold
        case 0x6:   if(ds_values[0]>near_th)
                        turnRight(M_PI);
                    else //当特别接近障碍时 转动改为平动
                        tranRight(M_PI);
                    break;

        case 0x1:   //0x1表明右边距离传感器测量的距离小于threshold，0x3表明右边和中间的距离传感器测距都小于threshold
        case 0x3:   if(ds_values[2]>near_th)
                        turnLeft(M_PI);
                    else
                        tranLeft(M_PI);
                    break;

                    //0x2表明只有中间的距离传感器测量的距离小于threshold 在这种情况下，往左边平动
        case 0x2:   tranLeft(1.5*M_PI);
                    break;
    }
    limitWheelSpeed();
}

/***************************************
 * 函数功能: 机械臂逆运动学
 * 参数: x y z 相对车坐标系的笛卡尔空间坐标; angle1 机械手腕相对xy平面的夹角 往上为正; angle2 最后一个关节轴的角度
 * 返回值: 逆解算成功返回1 否则返回0
 * 其他：连接不成功直接退出程序
 * *************************************/
int arm_ik(float x, float y, float z,float angle1,float angle2) 
{
    float x1,x2,z2,x3,z3,a,b,c,alpha,beta,gamma,delta,epsilon;
    
    x -= ARMXOFFSET;
    x1 = sqrt(x * x + y * y) ;
    
    x2 = fabs(x1) - (ARM5LENGTH+ARM4LENGTH)*cos(angle1);
    z2 = z - (ARM5LENGTH+ARM4LENGTH)*sin(angle1);
    
    z3 = (z2-ARM1VLENGTH);
    
    a = ARM2LENGTH;
    b = ARM3LENGTH;
    if(x>=0) //表明机械臂往正前方伸展
    {
        x3 = (x2-ARM1HLENGTH);

        c = sqrt(x3 * x3 + z3 * z3);
    
        if(a+b<c)
        {   
            printf("\n");
            printf("!!!Arm are unable to reach the target point\r\n!!!");
            printf("\n");
            return 0;
        }
    
        alpha = atan2(y , x); 
        beta = -(M_PI_2 - acos((a * a + c * c - b * b) / (2.0 * a * c)) - atan2(z3 , x3));
        gamma = -(M_PI - acos((a * a + b * b - c * c) / (2.0 * a * b)));
        delta = -(beta + gamma)-(M_PI_2-angle1);
        epsilon = angle2;
    }
    else//表明机械臂往正后方伸展
    {
        x3 = (x2+ARM1HLENGTH);
        
        c = sqrt(x3 * x3 + z3 * z3);
    
        if(a+b<c)
        {
            printf("\n");
            printf("!!!Arm are unable to reach the target point\r\n!!!");
            printf("\n");
            return 0;
        }
    
        alpha = atan2(y , x); 
        if(alpha > M_PI_2)
            alpha -= M_PI;
        else if(alpha <- M_PI_2)
            alpha += M_PI;
        
        beta = (M_PI_2 - acos((a * a + c * c - b * b) / (2.0 * a * c)) - atan2(z3 , x3));
        gamma = (M_PI - acos((a * a + b * b - c * c) / (2.0 * a * b)));
        delta = -((beta + gamma)-(M_PI_2-angle1));
        epsilon = angle2;
    }
    
    if(alpha<-2.95 || alpha>2.95)
    {
        printf("The joint 1 is out of range\r\n");
        return 0;
    }
    if(beta<-1.13 || beta>1.57)
    {
        printf("The joint 2 is out of range\r\n");
        return 0;
    }
    if(gamma<-2.64 || gamma>2.55)
    {
        printf("The joint 3 is out of range\r\n");
        return 0;
    }
    if(delta<-1.78 || delta>1.78)
    {
        printf("The joint 4 is out of range\r\n");
        return 0;
    }
    //printf("joint = %f %f %f %f %f\n",alpha,beta,gamma,delta,epsilon);

    if(fabs(angleDifference(angle1,-M_PI_2))<0.001) //如果手臂与地面呈-90度 则手臂侧角不随alpha角变化
         epsilon = alpha;                           //这是为视觉引导抓取而设置的，在机械臂沿着x轴运动时，需要让手腕部分不转动，具体看运动过程就能明白
    
    setArmJoint(alpha,beta,gamma,delta,epsilon);
    return 1;
}

/***************************************
 * 函数功能: 控制逻辑 先进行角度控制(对应原地旋转) 然后再进行位置控制(直线到达目标点) 到达目标位置后再进行角度控制
 * 返回值：到达目标位置和目标角度返回1 否则返回0
 * 其他：在到达目标点后再进行角度控制会使得位置有所偏移(未解决)
 * *************************************/
int controlLog(int sockid)
{
    int ret = 0;

    bzero(&ws,sizeof(ws));

    switch(control_flag)
    {
        //进行角度控制 目标角度为当前位置直线到达目标位置所需要的角度
        case 0: temp_tar_angle = atan2(tar_var.gpsX-cur_var.gpsX,tar_var.gpsZ-cur_var.gpsZ)+M_PI_2;
                if(angleControl(temp_tar_angle))
                    control_flag = 1;
                break;

        //同时进行位置和角度控制，目标角度为当前位置直线到达目标位置所需要的角度
        case 1: temp_tar_angle = atan2(tar_var.gpsX-cur_var.gpsX,tar_var.gpsZ-cur_var.gpsZ)+M_PI_2;
                if(positionControl(0))
                {
                    resetWheelSpeed();
                    sendCom(sockid,0,9,(uint32_t *)&ws,4);
                    control_flag = 2;
                    break;
                }  
                angleControl(temp_tar_angle);
                obstacleAvoidance();
                break;
        //到达目标点后控制角度到设定的目标角度
        case 2: temp_tar_angle = tar_var.angle;
                if(angleControl(temp_tar_angle))
                {
                    control_flag = 3;
                    ret = 1;
                }  
                break;
    }

    sendCom(sockid,0,9,(uint32_t *)&ws,4);
    return ret;
}
/***************************************
 * 函数功能: 车+机械臂位置控制 到达目标点进行视觉抓取 运送到第二个目标点
 * 参数: sockid 网路读写描述符
 * 返回值: 完成动作返回1 否则返回0
 * *************************************/
int armWithPosControl(int sockid)
{
    int ret = 0;

    bzero(&ws,sizeof(ws));
    switch(armwithpos_flag)
    {   

        case 0: if(controlLog(sockid)) //到达目标点
                    armwithpos_flag = 2;
                break;   

        case 2:
                arm_ik(armtar.x,armtar.y,armtar.z,armtar.angle1,0); //机械臂运动
                sendCom(sockid,0,10,(uint32_t *)&arm1,5);
                sleep(7);
                armwithpos_flag = 3;
                break;

        case 3: if(visualGrab(sockid) == 2) //视觉引导
                    armwithpos_flag = 4;
                break;

        case 4: if(grab(GRIPPERFORCE,sockid)) //抓取物体
                    armwithpos_flag = 5;
                break;

        case 5: arm_ik(-0.15,0,0.07,-M_PI_2/4,0);  //将物体放到车身平台
                sendCom(sockid,0,10,(uint32_t *)&arm1,5);
                sleep(7);
                armwithpos_flag = 6;
                break;

        case 6 ://printf("release gripper\n");
                gripper.gap = MAXGAP;   //松开夹爪
                sendCom(sockid,0,11,(uint32_t *)&gripper.gap,1);
                armwithpos_flag = 7;
                receiveTarget(0); //获取第二个目标点
                break;

        case 7 ://printf("second angle\n");
                temp_tar_angle = atan2(tar_var.gpsX-cur_var.gpsX,tar_var.gpsZ-cur_var.gpsZ)+M_PI_2;
                if(angleControl(temp_tar_angle))
                    armwithpos_flag = 8;
                break;

        case 8: //printf("second pos\n");
                temp_tar_angle = atan2(tar_var.gpsX-cur_var.gpsX,tar_var.gpsZ-cur_var.gpsZ)+M_PI_2;
                if(positionControl(0)) //运动到第二个目标点
                {
                    resetWheelSpeed();
                    sendCom(sockid,0,9,(uint32_t *)&ws,4);
                    armwithpos_flag = 9;
                    break;
                }   
                angleControl(temp_tar_angle);
                obstacleAvoidance();
                break;

        case 9: //printf("second grap\n");
                if(grab(GRIPPERFORCE,sockid)) //抓取车身平台上的物体
                    armwithpos_flag = 10;
                break;

        case 10:arm_ik(0.4,0,-0.06,-M_PI_2,0); 
                sendCom(sockid,0,10,(uint32_t *)&arm1,5); //将物体放到地面
                sleep(7);
                armwithpos_flag = 11;
                break;

        case 11:gripper.gap = MAXGAP;
                sendCom(sockid,0,11,(uint32_t *)&gripper.gap,1);//松开夹爪
                armwithpos_flag = 12;
                break;
        case 12: initArm(); //机械臂回到初始位置
                 sendCom(sockid,0,10,(uint32_t *)&arm1,5);
                 armwithpos_flag = 13;
                 break;
    }

    if(armwithpos_flag==7 || armwithpos_flag==8)
        sendCom(sockid,0,9,(uint32_t *)&ws,4);    
    return ret;
}
/***************************************
 * 函数功能: 视觉引导抓取
 * 参数: sockid 网路读写描述符
 * 返回值: 引导成功1 否则返回0
 * *************************************/
int visualGrab(int sockid)
{

    float dis_X,dis_Y,dis_Z,temp_X,temp_Y,temp_Z;

    if(cma_cur.cameraX == 0 || cma_cur.cameraY == 0 || cma_cur.cameraZ == 0)
    {
        //printf("There is no objects\n");
        sendCom(sockid,0,0,NULL,0);
        return 0;
    }

    dis_X = cma_tar.cameraX - cma_cur.cameraX;
    dis_Y = cma_tar.cameraY - cma_cur.cameraY;
    dis_Z = cma_tar.cameraZ - cma_cur.cameraZ;

    if((fabs(dis_X) <= DEADCAMERA) && (fabs(dis_Y) <= DEADCAMERA) && (fabs(dis_Z) <= DEADCAMERA))
    {
        sendCom(sockid,0,0,NULL,0);
        return 2;
    }
        
    else
    {
        if(fabs(dis_X) > DEADCAMERA)
            temp_X = dis_X*con.camera_I;
        else
            temp_X = 0;
        
        if(fabs(dis_Y) > DEADCAMERA)
            temp_Y = dis_Y*con.camera_I;
        else
            temp_Y = 0;
        
        if(fabs(dis_Z) > DEADCAMERA)
            temp_Z = dis_Z*con.camera_I;
        else
            temp_Z = 0;
    }
    
    fbound(&temp_X,-MAXARMENDSPEED,MAXARMENDSPEED);
    fbound(&temp_Y,-MAXARMENDSPEED,MAXARMENDSPEED);
    fbound(&temp_Z,-MAXARMENDSPEED,MAXARMENDSPEED);
    
    //改变机械臂末端位置
    armtar.x -= temp_Y;
    armtar.y += temp_X;
    armtar.z -= temp_Z;

    //printf("armtar %f %f %f\n",armtar.x,armtar.y,armtar.z);
    arm_ik(armtar.x,armtar.y,armtar.z,-M_PI_2,0);
    
    sendCom(sockid,0,10,(uint32_t *)&arm1,5);

    return 1;
}

/***************************************
 * 函数功能: 抓取
 * 参数: force 给定抓取力；sockid 网路读写描述符
 * 返回值: 抓取成功1 否则返回0
 * *************************************/
int grab(float force,int sockid)
{
    int ret = 0;

    if(gripper.force<force) //抓取力小于给定力 减小夹爪间隙
    {
        if(gripper.force < force-50)
            gripper.gap -= 0.0001;
        else
            gripper.gap -= 0.00001;

        gripper.gap = gripper.gap < MINGAP ? MINGAP : gripper.gap;
        sendCom(sockid,0,11,(uint32_t *)&gripper.gap,1);
    }
    else
    {
        ret = 1;
        sendCom(sockid,0,0,NULL,0);
    }    

    //printf("gab = %f\n",gripper.gap);
    return ret;
}

/***************************************
 * 函数功能: 从终端输入获得车体目标位置和角度
 * 参数：flag = 0 不重置控制标志；flag = 1 重置控制标志
 * 返回值: 成功获得返回1 否则返回0
 * *************************************/
int receiveTarget(int flag)
{
    float temp1,temp2,temp3;
    printf("Please input the destination,format:X Z Angle\r\n");
    while((scanf("%f %f %f",&temp1,&temp2,&temp3) != 3 ))
    {
        printf("Wrong input, input again\n");
        continue;
    }
    
    tar_var.gpsX = temp1;
    tar_var.gpsZ = temp2;
    tar_var.angle = temp3;

    if(flag)
    {
        control_flag = 0;
        armwithpos_flag = 0;
    }
    
    return 1;
}

/***************************************
 * 函数功能: 从终端输入获得机械臂目标位置和角度
 * 返回值: 成功获得返回1 否则返回0
 * *************************************/
int receiveArmTarget(void)
{
    float temp1,temp2,temp3;
    printf("Please input the destination,format:X Y Z\r\n");
    if((scanf("%f %f %f",&temp1,&temp2,&temp3) != 3 ))
        return 0;

    armtar.x = POSOFFSET;
    armtar.y = 0;
    armtar.z = temp2;

    if(armtar.z<=0.12)
        armtar.angle1 = -M_PI_2;
    else if(armtar.z<=0.2)
        armtar.angle1 = -M_PI_4;   
    else
        armtar.angle1 = 0;

    armtar.angle2 = 0;

    tar_var.gpsX = temp1;
    tar_var.gpsZ = temp3;

    armwithpos_flag = 0;
    return 1;
}

void initArmEnd(void)
{
    armtar.x = POSOFFSET;
    armtar.y = 0;
    armtar.z = 0.1;
    armtar.angle1 = -M_PI_2;
}

void resetWheelSpeed(void)
{
    ws.left_front = 0;
    ws.left_rear = 0;
    ws.right_front = 0;
    ws.right_rear = 0;
}

void limitWheelSpeed(void)
{
    fbound(&ws.left_front,-MAXWHEELSPEED,MAXWHEELSPEED);

    fbound(&ws.right_front,-MAXWHEELSPEED,MAXWHEELSPEED);

    fbound(&ws.left_rear,-MAXWHEELSPEED,MAXWHEELSPEED);

    fbound(&ws.right_rear,-MAXWHEELSPEED,MAXWHEELSPEED);
}

void goFoward(float speed)
{
    ws.right_front = ws.right_rear = ws.left_front = ws.left_rear = speed;
}

void turnLeft(float speed)
{
    ws.right_front = speed;
    ws.right_rear = speed;
    ws.left_front = -speed;
    ws.left_rear = -speed;
}

void turnRight(float speed)
{
    ws.right_front = -speed;
    ws.right_rear = -speed;
    ws.left_front = speed;
    ws.left_rear = speed;
}

void tranLeft(float speed)
{
    ws.right_front = speed;
    ws.right_rear = -speed;
    ws.left_front = -speed;
    ws.left_rear = speed;
}

void tranRight(float speed)
{
    ws.right_front = -speed;
    ws.right_rear = speed;
    ws.left_front = speed;
    ws.left_rear = -speed;
}